#include <string.h>
#include <stdio.h>
#include "main.h"
#include "drv_bsp.h"
#include "foc.h"
#include "as5600.h"
#include "algorithm.h"
#include "soft_i2c.h"
#include "pid.h"

#define IIC_SCL_GPIO_Port GPIO_PORT_A
#define IIC_SDA_GPIO_Port GPIO_PORT_A
#define IIC_SCL_Pin GPIO_PIN_03
#define IIC_SDA_Pin GPIO_PIN_04

void INT_SRC_TMRA_1_OVF_IrqCallback(void);
void INT_SRC_TMRA_2_OVF_IrqCallback(void);
void delay_nop(uint32_t x);
void scl_high(void) { GPIO_SetPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
void scl_low(void) { GPIO_ResetPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
void sda_high(void) { GPIO_SetPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }
void sda_low(void) { GPIO_ResetPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }
uint8_t scl_read(void) { return GPIO_ReadInputPins(IIC_SCL_GPIO_Port, IIC_SCL_Pin); }
uint8_t sda_read(void) { return GPIO_ReadInputPins(IIC_SDA_GPIO_Port, IIC_SDA_Pin); }

void set_va_pwm(uint32_t x){CM_TMRA_2->CMPAR1 = (uint16_t)(x);}
void set_vb_pwm(uint32_t x){CM_TMRA_2->CMPAR2 = (uint16_t)(x);}
void set_vc_pwm(uint32_t x){CM_TMRA_2->CMPAR3 = (uint16_t)(x);}

char str[128];
i2c_ops_t i2c_ops;
as5600_data_t as5600_data;
foc_t foc;
pid_t pid;
uint16_t u16Polarity[3] = {50, 50, 0};

// 假设dq轴电压指令和当前电角度
float Vd_f32 = 0.0f;    // d轴电压指令
float Vq_f32 = 0.0f;   // q轴电压指令
float theta_f32 = 0.0f; // 电角度（π/2弧度 = 90°）
float Valpha_f32, Vbeta_f32;
float Va_f32 = 0, Vb_f32, Vc_f32;

int Vd_q15 = 0;    // d轴电压指令
int Vq_q15 = 0;   // q轴电压指令
int theta_q15 = 0; // 电角度（π/2弧度 = 90°）
int Valpha_q15, Vbeta_q15;
int Va_q15 = 0, Vb_q15, Vc_q15;

float kp = 0.00,ki,op,oi,out;
int tangle = 0;
float kloi,fkloi;
float kj = 0;

int32_t svpwmd = 0;
int32_t svpwmq = 1;
int32_t svpwmtheta = 0;
int32_t svpwmalpha = 0;
int32_t svpwmbeta = 0;
int32_t svpwma = 0;
int32_t svpwmb = 0;
int32_t svpwmc = 0;

int32_t main(void)
{
  LL_PERIPH_WE(LL_PERIPH_ALL);
  dsp_drv_init();
  LL_PERIPH_WP(LL_PERIPH_ALL);

  i2c_ops.scl_high = scl_high;
  i2c_ops.scl_low = scl_low;
  i2c_ops.sda_high = sda_high;
  i2c_ops.sda_low = sda_low;
  i2c_ops.sda_read = sda_read;
  i2c_ops.delay = delay_nop;
  i2c_ops.delay_x = 1;
	
	as5600_init(&as5600_data,0.05/4.0);
	
	foc.set_va_pwm = set_va_pwm;
	foc.set_vb_pwm = set_vb_pwm;
	foc.set_vc_pwm = set_vc_pwm;
	foc.delay = DDL_DelayMS;
	foc.delay_x = 25;
	foc.initv = CM_TMRA_2->PERAR / 10;

	TMRA_Start(CM_TMRA_1);
  TMRA_Start(CM_TMRA_2);
	
	CM_TMRA_2->PCONR1 |= 0x01 << 12;
	CM_TMRA_2->PCONR2 |= 0x01 << 12;
	CM_TMRA_2->PCONR3 |= 0x01 << 12;
	
	DDL_DelayMS(100);
	
	foc.k_f32 = -0.021515917;
	foc.b_f32 = 45.8542595;
	foc.k_q15 = foc.k_f32 * 5215.18917524;
	foc.b_q15 = foc.b_f32 * 5215.18917524;
	
//	foc.ready = 1;
	
	foc_calibration(&foc,&as5600_data.angle,2048);
	
	pid.max_cout = 2498;
	pid.kp = 32768;
	
  TMRA_IntCmd(CM_TMRA_2, TMRA_INT_OVF, ENABLE);
	
  for (;;)
  {
			
  }
}


int kbj = 0,kjh = 1;

void INT_SRC_TMRA_2_OVF_IrqCallback(void)
{
	GPIO_SetPins(GPIO_PORT_B, GPIO_PIN_00);
	svpwmtheta = machinery_angle_to_electric_angle_q15(&foc,as5600_data.angle);
	svpwm_q15(svpwmd,svpwmq,svpwmtheta,&svpwma,&svpwmb,&svpwmc);
	GPIO_ResetPins(GPIO_PORT_B, GPIO_PIN_00);
	
	u16Polarity[0] = (uint16_t)(svpwma + 2499);
	u16Polarity[1] = (uint16_t)(svpwmb + 2499);
	u16Polarity[2] = (uint16_t)(svpwmc + 2499);

  CM_TMRA_2->CMPAR1 = (uint16_t)(u16Polarity[0]);
  CM_TMRA_2->CMPAR2 = (uint16_t)(u16Polarity[1]);
  CM_TMRA_2->CMPAR3 = (uint16_t)(u16Polarity[2]);
}

void INT_SRC_TMRA_1_OVF_IrqCallback(void)
{
  // add your codes here
  soft_i2c_readnByte(&i2c_ops, AS5600_ADDRESS_MAG, _stat, as5600_data.read_data, 1);
  as5600_status_analytical(&as5600_data);
  soft_i2c_readnByte(&i2c_ops, AS5600_ADDRESS_MAG, _ang_hi, as5600_data.read_data, 2);
  as5600_angle_analytical(&as5600_data,1000);
	if(foc.ready == 0)
		return;
	
//	tangle += kbj;
//	tangle %= 4096;
	
//	svpwmtheta+= kbj;
	
	pid_up(&pid,tangle,zero_processing(tangle,as5600_data.angle,2048));
//	pid_up(&pid,tangle,as5600_data.flp_angle_velocity.y);
	svpwmq = pid.cout * ((foc.k_q15 >= 0) ? (1) : (-1));
	
	sprintf(str, "%d,%d,%d\n", 
			svpwma,svpwmb,svpwmc
			);
			
	USART_UART_Trans(CM_USART1, str, strlen(str), 0xffff);
}

void delay_nop(uint32_t x)
{
  volatile uint32_t i;
  for (i = 0; i < x; i++)
  {
    __NOP();
  }
}